/**
 * 
 */
package program;

import java.util.Iterator;
import java.util.LinkedList;
import java.util.List;

import javax.vecmath.Point2d;

import robot.Robot;

/**
 * @date 7 maj 2012
 * @version 1.0
 */
public class MovingObjects {
	public static final double WITHIN_DISTANCE = 0.5;
	
	private List<Robot> movingObjects;
	
	public MovingObjects () {
		movingObjects = new LinkedList<Robot>();
	}
	
	public void addRobot (Robot r) {
		movingObjects.add(r);
	}
	
	public Point2d getPosCloseTo (Point2d pos) {
		for (Robot r : movingObjects) {
			Point2d rPos = r.getCurrentPosition(); 
			if (rPos.distance(pos) < WITHIN_DISTANCE) {
				return rPos; 
			}
		}
		
		return null;
	}
	
	public Point2d getDirCloseTo (Point2d pos) {
		for (Robot r : movingObjects) {
			Point2d rPos = r.getCurrentPosition(); 
			if (rPos.distance(pos) < WITHIN_DISTANCE) {
				double speed = r.getTranslationalVelocity();
				
				return new Point2d ( Math.cos(r.getHeading()) * speed,
									-1* Math.sin(r.getHeading()) * speed); 
			}
		}
		
		return null;
	}
	
	public Iterator<Robot> iterator() {
		return movingObjects.iterator();
	}
	
	public String toString () {
		return "Size: " + movingObjects.size();
	}

}
